#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_geometry/pinhole_camera_model.h>
#include <tf/transform_listener.h>
#include <boost/foreach.hpp>
using namespace cv;
class FrameDrawer
{
    public:
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::CameraSubscriber sub_;
  image_transport::Publisher pub_;
  cv_bridge::CvImage bridge_;
  std::vector<std::string> frame_ids_;
  CvFont font_;
  CvCapture* m_capture;
public:
  FrameDrawer(const std::vector<std::string>& frame_ids) : it_(nh_), frame_ids_(frame_ids)
  {
    std::string image_topic = nh_.resolveName("image_pub");
    pub_ = it_.advertise("cv_image_out", 2);
    cvInitFont(&font_, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5);
    bridge_.encoding="bgr8";
    m_capture = cvCaptureFromFile( "/home/viki/Videos/kick.avi" );
    //bridge_.image=imread("/home/viki/Pictures/ball.jpg");
  }

  bool imageCb()
  {
    bridge_.image=cvQueryFrame( m_capture );
    if(!bridge_.image.cols){
        ROS_ERROR("empty image");
        return false;
    }
    bridge_.header.stamp = ros::Time::now();
    pub_.publish(bridge_.toImageMsg());
    return true;
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "draw_frames");
  std::vector<std::string> frame_ids(argv + 1, argv + argc);
  FrameDrawer drawer(frame_ids);
  ros::spinOnce();
  ros::Rate p(0.5);
  while(ros::ok())
  {
    p.sleep();
    if(!drawer.imageCb())
        break;
  }
}
